#include "IDropletSim.h"

IDropletSim::IDropletSim()
{
	firstRun = true;
	droplets.clear();
}

IDropletSim::~IDropletSim()
{
	droplets.clear();
}

DS_RESULT IDropletSim::AddCollisionShape(btCollisionShape *colShape, int *colShapeIndex)
{
	simPhysics->collisionShapes->push_back(colShape);
	*colShapeIndex = simPhysics->_colShapeIDCounter++;

	return DS_SUCCESS;
}



DS_RESULT IDropletSim::CreateFloor(
	int floorShapeIndex,
	int wallXShapeIndex, 
	int wallYShapeIndex)
{
	// Set up the Floor
	ObjectPhysicsData floorData;
	floorData.mass = 0.0;	// Static object
	floorData.localInertia = btVector3(0, 0, 0);
	floorData.friction = 0.8f;
	floorData.colShapeIndex = floorShapeIndex;

	initPhysicsObject(&floorData, btVector3(0, 0, 0));

	// Set up walls - if requested
	/* Note that the Y-axis point "Up" in bullet so, we are actually building the wall along
	 * the x and z axes and not the x and y axes but for conformity with the rest of the 
	 * simulator, the variables are named using a Z-axis points "Up" coordinate system.
	 */
	if(simSetupData->autoBuildBoundaryWalls)
	{
		if(wallXShapeIndex < 0 || wallYShapeIndex < 0)
		{
			// TODO : Print warning message
			return DS_WARNING;
		}

		for(int i = 0; i < 2; i++)
		{
			btScalar startXPos = powf(-1.0, (float)i) * simSetupData->tileLength * simSetupData->numRowTiles / 2;
			btScalar startZPos = powf(-1.0, (float)i) * simSetupData->tileLength * simSetupData->numColTiles / 2;

			ObjectPhysicsData wallData;
			wallData.mass = 0.0f;	// Static object
			wallData.localInertia = btVector3(0, 0, 0);
			wallData.friction = 0.0f;
			wallData.colShapeIndex = wallYShapeIndex;
			initPhysicsObject(&wallData, btVector3(startXPos, 0, 0));

			wallData.colShapeIndex = wallXShapeIndex;
			initPhysicsObject(&wallData, btVector3(0, 0, startZPos));
		}
	}

	return DS_SUCCESS;
}


DS_RESULT IDropletSim::AddDroplet(		
	IDroplet *pDroplet, 
	std::pair<float, float>startPos,
	float startAngle)
{
	DS_RESULT retval = pDroplet->_InitPhysics(simPhysics, startPos, startAngle); // Set up physics for this droplet
	if(retval == DS_SUCCESS)
		droplets.push_back(pDroplet); 
	else
	{
		// TODO : Print Error message
	}
	return retval;
}

DS_RESULT IDropletSim::Init(const SimSetupData &setupData)
{
	DS_RESULT retval;

	// Set up simulator init parameters
	simSetupData = new SimSetupData(setupData);

	// Initialize Physics engine
	retval = initPhysics();

	return retval;
}

/*
 * The main body of the simulator goes here. The Update() method is responsible for
 * updating all state variables of the simulator and running the physics engine for
 * a single timestep. Order of operations performed in this method are organized
 * as follows :
 * 
 *
 */
DS_RESULT IDropletSim::Step()
{
	std::vector<IDroplet *>::iterator it;
	if(firstRun)
	{
		for(it = droplets.begin(); it != droplets.end(); it++)
		{
			IDroplet *pDroplet = *it;
			pDroplet->DropletInit();
		}
		firstRun = false;
	}

	for(it = droplets.begin(); it != droplets.end(); it++)
	{
		IDroplet *pDroplet = *it;
		pDroplet->DropletMainLoop();
	}

	motionController();

	// Step the physics simulation. Default run at 60fps = default for Bullet.
	// To understand how stepSimulation() works, 
	// ref : http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_The_World
	simPhysics->dynWorld->stepSimulation(1.0f / SIM_DEFAULT_FPS);

	return DS_SUCCESS;
}

DS_RESULT IDropletSim::Cleanup()
{
	return endPhysics(); // TODO : Should this be in the destrcutor on Cleanup()?

	delete simSetupData;
}

//DS_RESULT IDropletSim::Run()
//{
//	DS_RESULT retval = DS_SUCCESS;
//	
//	retval = Init();
//	if(retval != DS_SUCCESS)
//		return retval;
//
//	while(retval < DS_ERROR)	// Warnings are OK, but errors and fatal errors are not.
//	{
//		retval = Update();
//	}
//
//	retval = Cleanup();
//
//	return retval; 
//}

DS_RESULT IDropletSim::GetDropletPositions(std::vector<float *> *xyValues)
{

	// TODO : Error checking. Make sure xyValues have enough space allocated for storing data
	btCollisionObjectArray objs = simPhysics->dynWorld->getCollisionObjectArray();
	int i = 0;	
	std::vector<IDroplet *>::iterator it;
	for(it = droplets.begin(); it < droplets.end(); it++)
	{
		IDroplet *pDroplet = *it;
		ObjectPhysicsData *objPhysics;
		AccessPhysicsData(pDroplet, &objPhysics);

		btRigidBody *body = btRigidBody::upcast(objs[objPhysics->_worldID]);
		if (body && body->getMotionState())
		{
			btTransform trans;
			body->getMotionState()->getWorldTransform(trans);
			xyValues->at(i)[0] = static_cast<float>(trans.getOrigin().getX());
			xyValues->at(i)[1] = static_cast<float>(trans.getOrigin().getZ());
		}
		i++;
	}

	return DS_SUCCESS;
}

DS_RESULT IDropletSim::GetDropletColors(std::vector<uint8_t *> *colors)
{
	std::vector<IDroplet *>::iterator it;
	int i = 0;
	for(it = droplets.begin(); it < droplets.end(); it++)
	{
		IDroplet *pDroplet = *it;
		DropletActuatorData *actData;
		AccessActuatorData(pDroplet, &actData);

		colors->at(i)[0] = actData->rOut;
		colors->at(i)[1] = actData->gOut;
		colors->at(i)[2] = actData->bOut;
		i++;
	}

	return DS_SUCCESS;
}

DS_RESULT IDropletSim::initPhysics()
{
	simPhysics = (SimPhysicsData *)malloc(sizeof(SimPhysicsData));

	//collision configuration contains default setup for memory, collision setup
	simPhysics->collisionConfig		= new btDefaultCollisionConfiguration();
	
	// Use the default collision dispatcher. 
	// For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)	
	simPhysics->collisionDispatch	= new btCollisionDispatcher(simPhysics->collisionConfig);
	simPhysics->broadphase			= new btDbvtBroadphase();

	// The default constraint solver. 
	// For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)	
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;	
	simPhysics->constraintSolver	= sol;
	
	simPhysics->dynWorld			= new btDiscreteDynamicsWorld(
		simPhysics->collisionDispatch,
		simPhysics->broadphase,
		simPhysics->constraintSolver,
		simPhysics->collisionConfig
	);

	simPhysics->dynWorld->setGravity(btVector3(0, PHYSICS_GRAVITY, 0));
	simPhysics->_physicsWorldObjCounter = 0;

	// Create the Collision Shapes for the Droplets and Floor
	simPhysics->collisionShapes = new btAlignedObjectArray<btCollisionShape *>();
	simPhysics->collisionShapes->clear();
	simPhysics->_colShapeIDCounter = 0;

	return DS_SUCCESS;
}

DS_RESULT IDropletSim::endPhysics()
{
	// TODO : Delete simPhysics->collisionShapes object array
	delete simPhysics->dynWorld;
	delete simPhysics->constraintSolver;
	delete simPhysics->broadphase;
	delete simPhysics->collisionDispatch;
	delete simPhysics->collisionConfig;

	free(simPhysics);

	return DS_SUCCESS;
}

DS_RESULT IDropletSim::initPhysicsObject(ObjectPhysicsData *objPhysics, btVector3 &origin)
{
	btCollisionShape *colShape = simPhysics->collisionShapes->at(objPhysics->colShapeIndex);

	// Set object frame origin
	btTransform objTransform;
	objTransform.setIdentity();
	objTransform.setOrigin(origin);

	//rigidbody is dynamic if and only if objPhysics->mass is non zero, otherwise static
	bool isDynamic = (objPhysics->mass != 0.f);
	if (isDynamic)
		colShape->calculateLocalInertia(objPhysics->mass, objPhysics->localInertia);

	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(objTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(
		objPhysics->mass,
		myMotionState,
		colShape,
		objPhysics->localInertia);
	rbInfo.m_friction = objPhysics->friction;
	btRigidBody* body = new btRigidBody(rbInfo);

	//add the body to the dynamics world
	simPhysics->dynWorld->addRigidBody(body);
	simPhysics->_physicsWorldObjCounter++;

	return DS_SUCCESS;
}


// ----- Control and Unpdate Functions ----- //

void IDropletSim::motionController()
{
	btCollisionObjectArray objs = simPhysics->dynWorld->getCollisionObjectArray();

	std::vector<IDroplet *>::iterator it;
	for(it = droplets.begin(); it < droplets.end(); it++)
	{
		IDroplet *pDroplet = *it;
		DropletActuatorData *actData;
		AccessActuatorData(pDroplet, &actData);

		// Replace 

		// Move/Rotate all the droplets
		if(actData->moveTimeRemaining > 0 || actData->rotateTimeRemaining > 0)
		{
			ObjectPhysicsData *objPhysics;
			AccessPhysicsData(pDroplet, &objPhysics);
			btRigidBody *body = btRigidBody::upcast(objs[objPhysics->_worldID]);
		
			if(actData->moveTimeRemaining > 0)
			{
				move_direction md = actData->currMoveDir;
				btVector3 impulseVec;
				switch(md)
				{
				case NORTH :
					impulseVec = btVector3(1.0f, 0.0f, 0.0f);
					break;

				case NORTH_EAST :
					impulseVec = btVector3(0.707f, 0.0f, -0.707f);
					break;

				case SOUTH_EAST :
					impulseVec = btVector3(-0.707f, 0.0f, -0.707f);
					break;

				case SOUTH :
					impulseVec = btVector3(-1.0f, 0.0f, 0.0f);
					break;

				case SOUTH_WEST :
					impulseVec = btVector3(-0.707f, 0.0f, 0.707f);
					break;

				case NORTH_WEST :
					impulseVec = btVector3(0.707f, 0.0f, 0.707f);
					break;

				default :
					impulseVec = btVector3(0.0f, 0.0f, 0.0f);
					break;
				}

				body->applyCentralImpulse(impulseVec);
				actData->moveTimeRemaining -= simSetupData->timestep;
			}

			if(actData->rotateTimeRemaining > 0)
			{
				// TODO : Figure out the best way to simulate rotation
			}
		}
	}
}




// ====== SimSetupData Class Functions ====== //

SimSetupData::SimSetupData(
	int numRowTiles,
	int numColTiles,
	float tileLength,
	float dropletRadius,
	float fps,
	bool autoBuildBoundaryWalls)
{
	this->fps = fps;
	this->timestep = 1000.0f / this->fps;
	this->numRowTiles = numRowTiles;
	this->numColTiles = numColTiles;
	this->tileLength  = tileLength;
	this->dropletRadius = dropletRadius;
	this->autoBuildBoundaryWalls = autoBuildBoundaryWalls;
}

SimSetupData::SimSetupData(const SimSetupData& setupData)
{
	this->fps = setupData.fps;
	this->timestep = setupData.timestep;
	this->numRowTiles = setupData.numRowTiles;
	this->numColTiles = setupData.numColTiles;
	this->tileLength  = setupData.tileLength;
	this->dropletRadius = setupData.dropletRadius;
	this->autoBuildBoundaryWalls = setupData.autoBuildBoundaryWalls;
}